﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

using System.Xml;

namespace VehicleTest
{
    class VehicleCommand
    {
        private MainWindows mainwindows;
        private XmlDocument xml_readVehicleParameters = new XmlDocument();

        public int SPEED_MAX;
        public int SPEED_MIN;
        public int SERVO_MAX;
        public int SERVO_MID;
        public int SERVO_MIN;
        public int SERVO_Cali;

        public VehicleCommand(MainWindows mw)
        {
            this.mainwindows = mw;

            //SPEED_MAX = 4500;
            //SPEED_MIN = -4500;
            //SERVO_MAX = 864;
            //SERVO_MID = 576;
            //SERVO_MIN = 344;

            ParameterRead();
        }
        
        private void ParameterRead()
        {
            xml_readVehicleParameters.Load(mainwindows.path + "/doc/VehicleParameters.xml");
            XmlNode currentNode;

            //加载舵机和电机参数
            currentNode = xml_readVehicleParameters.SelectSingleNode("//ServoMin");
            SERVO_MIN = Convert.ToInt32(currentNode.InnerText);

            currentNode = xml_readVehicleParameters.SelectSingleNode("//ServoMax");
            SERVO_MAX = Convert.ToInt32(currentNode.InnerText);

            currentNode = xml_readVehicleParameters.SelectSingleNode("//ServoMid");
            SERVO_MID = Convert.ToInt32(currentNode.InnerText);

            currentNode = xml_readVehicleParameters.SelectSingleNode("//ServoCalibration");
            SERVO_Cali = Convert.ToInt32(currentNode.InnerText);

            currentNode = xml_readVehicleParameters.SelectSingleNode("//MotorMax");
            SPEED_MAX = Convert.ToInt32(currentNode.InnerText);

            currentNode = xml_readVehicleParameters.SelectSingleNode("//MotorMin");
            SPEED_MIN = Convert.ToInt32(currentNode.InnerText);
        }

        /***************************************/
        //函数名：ConvertToPacket_Vehicle
        //函数功能：将相关参数转为小车控制指令包
        //入口参数：data	   为指针，指向所发数据包数组的第一个数据（数组名）,数组长度至少为10；
        //          mode       指令功能模式，0x00：发送电机速度和舵机偏转信息，此时只利用speed和servo_del； 
        //									 0x01：只发送速度信息，此时只利用speed；
        //									 0x02：只发送舵机偏转角信息，此时只利用servo_del；
        //									 0x03：发送控制红外传感器信息，此时只利用light；
        //			speed	   电机速度，范围从-4500~+4500
        //			servo_del  舵机偏转信息，范围从-228~+268，负表示右转，正表示左转
        //			light	   控制小车的红外传感器开关，八位数，一位控制一个红外，为0时为开状态，为1时为关状态
        //出口参数：length	   数据包长度
        public int ConvertToPacket_Vehicle(byte[]data, byte mode, int speed, int servo_del, byte light)
        {
            int length = 0;
            byte spd_h = 0;
            byte spd_l = 0;
            byte dir_mode = 0;
            byte svo_del_h = 0;
            byte svo_del_l = 0;
            byte check_sum = 0;

            servo_del += SERVO_Cali; //校正舵机

            switch (mode)
            {
                case 0x00:  //发送电机速度和舵机偏转信息，此时只利用speed和servo_del
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x07;
                    data[3] = 0x00;
                    if ((speed >= 0) && (servo_del >= 0))
                    {
                        dir_mode = 0x00;
                    }
                    else if ((speed < 0) && (servo_del >= 0))
                    {
                        dir_mode = 0x01;
                        speed = -speed;
                    }
                    else if ((speed >= 0) && (servo_del < 0))
                    {
                        dir_mode = 0x02;
                        servo_del = -servo_del;
                    }
                    else if ((speed < 0) && (servo_del < 0))
                    {
                        dir_mode = 0x03;
                        speed = -speed;
                        servo_del = -servo_del;
                    }

                    spd_h = (byte)(speed >> 8);
                    spd_l = (byte)(speed & 0x00FF);
                    svo_del_h = (byte)(servo_del >> 8);
                    svo_del_l = (byte)(servo_del & 0x00FF);

                    data[4] = dir_mode;
                    data[5] = spd_h;
                    data[6] = spd_l;
                    data[7] = svo_del_h;
                    data[8] = svo_del_l;
                    length = 10;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[9] = check_sum;

                    break;

                case 0x01:  //只发送速度信息，此时只利用speed
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x05;
                    data[3] = 0x01;
                    if ((speed >= 0) && (servo_del >= 0))
                    {
                        dir_mode = 0x00;
                    }
                    else if ((speed < 0) && (servo_del >= 0))
                    {
                        dir_mode = 0x01;
                        speed = -speed;
                    }
                    else if ((speed >= 0) && (servo_del < 0))
                    {
                        dir_mode = 0x02;
                        servo_del = -servo_del;
                    }
                    else if ((speed < 0) && (servo_del < 0))
                    {
                        dir_mode = 0x03;
                        speed = -speed;
                        servo_del = -servo_del;
                    }

                    spd_h = (byte)(speed >> 8);
                    spd_l = (byte)(speed & 0x00FF);

                    data[4] = dir_mode;
                    data[5] = spd_h;
                    data[6] = spd_l;
                    length = 8;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[7] = check_sum;

                    break;

                case 0x02:  //只发送舵机偏转角信息，此时只利用servo_del
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x05;
                    data[3] = 0x02;
                    if ((speed >= 0) && (servo_del >= 0))
                    {
                        dir_mode = 0x00;
                    }
                    else if ((speed < 0) && (servo_del >= 0))
                    {
                        dir_mode = 0x01;
                        speed = -speed;
                    }
                    else if ((speed >= 0) && (servo_del < 0))
                    {
                        dir_mode = 0x02;
                        servo_del = -servo_del;
                    }
                    else if ((speed < 0) && (servo_del < 0))
                    {
                        dir_mode = 0x03;
                        speed = -speed;
                        servo_del = -servo_del;
                    }

                    svo_del_h = (byte)(servo_del >> 8);
                    svo_del_l = (byte)(servo_del & 0x00FF);

                    data[4] = dir_mode;
                    data[5] = svo_del_h;
                    data[6] = svo_del_l;
                    length = 8;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[7] = check_sum;

                    break;

                case 0x03:  //发送控制红外传感器信息，此时只利用light
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x03;
                    data[3] = 0x03;
                    data[4] = light;
                    length = 6;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[5] = check_sum;
                    break;

                case 0x04: //急停指令,用于遥控时
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x02;
                    data[3] = 0x04;
                    length = 5;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[4] = check_sum;
                    break;

                case 0x05://急停恢复指令,用于遥控时
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x02;
                    data[3] = 0x05;
                    length = 5;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[4] = check_sum;
                    break;

                case 0x06://重置指令
                    data[0] = 0xFF;
                    data[1] = 0xFF;
                    data[2] = 0x02;
                    data[3] = 0x06;
                    length = 5;
                    for (int i = 2; i <= (length - 2); i++)
                    {
                        check_sum += data[i];
                    }
                    check_sum = (byte)~((int)check_sum);
                    data[4] = check_sum;
                    break;

                default:

                    break;
            }

            return length;
        }

        public void VehicleControl(byte mode, int speed, int servo_del, byte light)
        {
            byte[] data = new byte[255];
            int length = 0;

            length = ConvertToPacket_Vehicle(data, mode, speed, servo_del, light);
            mainwindows.SerialSend2(data, length);

            string show = null;
            show = mainwindows.ByteArrayToHexString_length(data, length);

            mainwindows.richTextBox_DataShow.Invoke(new EventHandler(delegate
            {
                mainwindows.richTextBox_DataShow.SelectionColor = Color.Green;
                mainwindows.richTextBox_DataShow.AppendText(show + "\n");
                mainwindows.richTextBox_DataShow.ScrollToCaret();
            }));

        }
    }
}
